﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace BoardController.Helper
{
    partial class BoardController
    {
        private IMU leftimu;
        private IMU rightimu;
        public VitualDevice vitual;
        public TagCollection tagCollection;
        //private AsyncServer netServer;

        //private LightPort leftlightPort;
        //private LightPort rightlightPort;

        public Boolean putterFlag;
        private PID pid;
        List<Device> Fandevices;
        private int Fandelay;
        private CancellationTokenSource FancancelTokenSource = new CancellationTokenSource();
        private bool autoFanFlag = false;



        public void FanController(List<Device> _devices, int _delay)
        {
            vitual = (VitualDevice)_devices[0];
            leftimu = (IMU)_devices[1];
            rightimu = (IMU)_devices[2];
            //tagCollection = (TagCollection)_devices[3];

            Fandevices = _devices;
            Fandelay = _delay;

            autoFanFlag = false;
        }

        public enum PumpControlType
        {
            Stop,
            Forward,
            Reverse
        }

        public enum PutterControlType
        {
            Stop,
            Forward,
            Reverse
        }

        public enum SilpControlType
        {
            LEFT,
            RIGHT
        }

        /// <summary>
        ///供料泵控制 speed 范围为 0~20000, 不填写默认为0
        /// </summary>
        /// <param name="toopen"></param>
        /// <param name="speed"></param>
        public void PumpControl(PumpControlType toopen, int speed = 0)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x1E;
            int RegsCount = 1;
            List<int> data = new List<int>();
            switch (toopen)
            {
                case PumpControlType.Forward:
                    data.Add(1 * 256 + speed);
                    break;
                case PumpControlType.Stop:
                    data.Add(0);
                    break;
                case PumpControlType.Reverse:
                    data.Add(2 * 256 + speed);
                    break;
                default:
                    Console.WriteLine("PumpControlType is illegal!");
                    break;
            }
            //data.Add(speed);
            SendMsg(addrDev, function, addrReg, RegsCount, data, 1);
        }

        /// <summary>
        /// 闭环控制泵
        /// </summary>
        /// <param name="delay"></param>
        /// <param name="pumpSeed"></param>
        /// <returns></returns>
        public bool PumpOpen(int delay, int pumpSeed)
        {
            int count = 0;
            LogHelper.WriteLog("Start open pump!");
            // 下位机返回的开泵标志
            while (leftimu.PumpFlag != 1)
            {
                PumpControl(PumpControlType.Forward, pumpSeed);
                Thread.Sleep(delay);
                count++;
                if (count > 4)
                {
                    LogHelper.WriteLog("open pump occur error!");
                    return false;
                }
            }
            LogHelper.WriteLog("Open pump successful!");
            return true;
        }

        public bool PumpClose(int delay)
        {
            int count = 0;
            LogHelper.WriteLog("Start close pump!");
            // 下位机返回的关泵标志
            while (leftimu.PumpFlag != 0)
            {
                PumpControl(PumpControlType.Stop);
                Thread.Sleep(delay);
                count++;
                if (count > 4)
                {
                    LogHelper.WriteLog("close pump occur error!");
                    return false;
                }
            }
            LogHelper.WriteLog("close pump successful!");
            return true;
        }

        /// <summary>
        /// 推杆电机控制
        /// </summary>
        /// <param name="toopen"></param>
        /// <param name="speed"></param>
        public void PutterControl(int putterType, PutterControlType toopen, int dir)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x20;
            int RegsCount = 2;
            List<int> data = new List<int>();
            switch (toopen)
            {
                case PutterControlType.Forward:
                    data.Add(putterType * 256 + 1);
                    break;
                case PutterControlType.Stop:
                    data.Add(0);
                    break;
                case PutterControlType.Reverse:
                    data.Add(putterType * 256 + 2);
                    break;
                default:
                    Console.WriteLine("PutterControlType is illegal!");
                    break;
            }
            data.Add(dir);
            SendMsg(addrDev, function, addrReg, RegsCount, data, 1);
        }

        /// <summary>
        /// 滑台控制(1;复位（无效）2：速度模式 dis:1表示左移 2表示右移 3：位置模式 dis表示位置参数，0-8000，对应位置0-8000mm 4:急停 （无效） )
        /// </summary>
        /// <param name="toopen"></param>
        /// <param name="speed"></param>
        public void SlipControl(int slipType, int dis, int speed)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x13;
            int RegsCount = 3;

            List<int> data = new List<int>();
            data.Add(slipType);
            data.Add(dis);
            data.Add(speed * 256);

            SendMsg(addrDev, function, addrReg, RegsCount, data, 1);
        }

        /// <summary>
        /// 滑台急停
        /// </summary>
        /// <param name="toopen"></param>
        /// <param name="speed"></param>
        public void SlipStop()
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x13;
            int RegsCount = 1;
            List<int> data = new List<int>();
            data.Add(4);
            SendMsg(addrDev, function, addrReg, RegsCount, data, 1);
        }

        /// <summary>
        /// 风扇控制
        /// </summary>
        /// <param name="toopen"></param>
        /// <param name="speed"></param>
        public void FanControl(int windSpeed)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x18;
            int RegsCount = 1;
            List<int> data = new List<int>();
            data.Add(windSpeed * 256 + windSpeed);
            SendMsg(addrDev, function, addrReg, RegsCount, data, 1);
        }



        #region SetUp Region
        public bool AutoFanFlag { get { return autoFanFlag; } set { autoFanFlag = value; } }



        public void SendMsg(int addrDev, int function, int addrReg, int RegsCount, List<int> data, int label)
        {
            netServer.Send(addrDev, function, addrReg, RegsCount, data, label);
        }

        public void FanStartAuto()
        {
            //OpenConnection();
            Task.Factory.StartNew(FanAutoTask, FancancelTokenSource.Token);
        }

        public void FanStopAuto()
        {
            FancancelTokenSource.Cancel();
            StopFans();
            CloseConnection();
        }
        #endregion


        #region Read Region

        private void FanAutoTask()
        {
            Thread.Sleep(50);
            SelfChecking();
            while (!FancancelTokenSource.IsCancellationRequested)
            {
                AskLeftIMU();
                Thread.Sleep(delay);
                AskRightIMU();
                Thread.Sleep(delay);
            }
            FancancelTokenSource = new CancellationTokenSource();
        }

        private void SelfChecking()
        {
            List<double> left_yaw = new List<double>();
            List<double> right_yaw = new List<double>();
            List<double> left_pitch = new List<double>();
            List<double> right_pitch = new List<double>();

            List<double> left_roll = new List<double>();
            List<double> right_roll = new List<double>();

            for (int i = 0; i < 100; i++)
            {
                AskLeftIMU();
                Thread.Sleep(delay);
                AskRightIMU();
                Thread.Sleep(delay);

                left_yaw.Add(leftimu.Yaw);
                right_yaw.Add(rightimu.Yaw);
                left_pitch.Add(leftimu.Pitch);
                right_pitch.Add(rightimu.Pitch);

                left_roll.Add(leftimu.Roll);
                right_roll.Add(rightimu.Roll);
            }

            vitual.YawBase = (left_yaw.Average() + right_yaw.Average()) / 2;
            vitual.PitchBase = (left_pitch.Average() + right_pitch.Average()) / 2;
            vitual.RollBase = (left_roll.Average() + right_roll.Average()) / 2;

        }

        private void FanUpdateDevice(Msg msg)
        {
            foreach (var device in devices)
            {
                device.Update(msg);
            }
            if (Manifest.Emergency)
            {
                AutoFanFlag = false;
            }
            //to do 
            //pid.Run(AutoFanFlag, 1);
        }


        public void AskLeftIMU()
        {
            int addrDev = 0x04;
            int function = 0x03;
            int addrReg = 0x81;
            //int count = 18;
            int count = 21;
            SendMsg(addrDev, function, addrReg, count, new List<int>(), 1);
        }

        public void AskRightIMU()
        {
            int addrDev = 0x05;
            int function = 0x03;
            int addrReg = 0x81;
            //int count = 18;
            int count = 21;
            SendMsg(addrDev, function, addrReg, count, new List<int>(), 1);
        }
        #endregion


        #region Write Region
        public void StopFans()
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x10;
            int RegsCount = 6;

            int left = pid.FanData[0];
            int right = pid.FanData[1];
            int frontleft = pid.FanData[2];
            int frontright = pid.FanData[3];
            int backleft = pid.FanData[4];
            int backright = pid.FanData[5];

            int offset = 8;
            int size = 80;
            int count = size / offset + 1;
            int least = 15;

            //for (int i = 0; i <count; i++)
            //{
            //    left = Math.Max(left - offset, 0);
            //    right = Math.Max(right - offset, 0);
            //    frontleft = Math.Max(frontleft - offset, least);
            //    Console.WriteLine(frontleft);
            //    frontright = Math.Max(frontright - offset, 0);
            //    backleft = Math.Max(backleft - offset, 0);
            //    backright = Math.Max(backright - offset, 0);

            //    List<int> data = new List<int>();
            //    data.Add(left);
            //    data.Add(right);
            //    data.Add(frontleft);
            //    data.Add(frontright);
            //    data.Add(backleft);
            //    data.Add(backright);

            //    SendMsg(addrDev, function, addrReg, RegsCount, data, 1);

            //    Thread.Sleep(100);                
            //}

            List<int> data = new List<int>();
            data.Add(0);
            data.Add(0);
            data.Add(0);
            data.Add(0);
            data.Add(0);
            data.Add(0);

            SendMsg(addrDev, function, addrReg, RegsCount, data, 1);

        }

        public void SilenceFans()
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x10;
            int RegsCount = 6;

            List<int> data = new List<int>();
            data.Add(20);
            data.Add(20);
            data.Add(15);
            data.Add(15);
            data.Add(10);
            data.Add(10);

            SendMsg(addrDev, function, addrReg, RegsCount, data, 1);
        }

        public void ControllLeft(int value)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x10;
            int count = 1;
            List<int> data = new List<int>();
            data.Add(value);

            SendMsg(addrDev, function, addrReg, count, data, 1);
        }

        public void ControllRight(int value)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x11;
            int count = 1;
            List<int> data = new List<int>();
            data.Add(value);

            SendMsg(addrDev, function, addrReg, count, data, 1);
        }

        public void ControllFrontLeft(int value)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x12;
            int count = 1;
            List<int> data = new List<int>();
            data.Add(value);

            SendMsg(addrDev, function, addrReg, count, data, 1);
        }

        public void ControllFrontRight(int value)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x13;
            int count = 1;
            List<int> data = new List<int>();
            data.Add(value);

            SendMsg(addrDev, function, addrReg, count, data, 1);
        }

        public void ControllBackLeft(int value)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x14;
            int count = 1;
            List<int> data = new List<int>();
            data.Add(value);

            SendMsg(addrDev, function, addrReg, count, data, 1);
        }


        public void ControllBackRight(int value)
        {
            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x15;
            int count = 1;
            List<int> data = new List<int>();
            data.Add(value);

            SendMsg(addrDev, function, addrReg, count, data, 1);
        }
        #endregion


        #region  Data Region

        public int[] FanData { get { return pid.FanData.ToArray(); } }

        public string[] LeftIMUStr { get { return leftimu.Values(); } }
        public string[] rightIMUStr { get { return rightimu.Values(); } }


        public string[] VitualDataStr { get { return vitual.Values(); } }

        public string[] LatestMsg
        {
            get
            {

                if (netServer != null)
                {
                    return netServer.LatestMsg;
                }
                else
                {
                    return new string[] { "111", "222" };
                }


            }
        }




        #endregion


    }
}
